#include <tasks_manager/task_manager_node.h>

/*******************************************
 * 获取机器人位姿
 *******************************************/
// 获取TF位姿数据
bool TaskManagerNode::GetTFPose(string father_id, string child_id, geometry_msgs::PoseStamped& pose_stamp, ros::Time time){
    // 获取父坐标系到子坐标系的位姿
    geometry_msgs::TransformStamped transform;
    try{
        transform = __tf_buffer->lookupTransform(father_id, child_id, time);
        pose_stamp.header = transform.header;
        pose_stamp.pose.position.x = transform.transform.translation.x;
        pose_stamp.pose.position.y = transform.transform.translation.y;
        pose_stamp.pose.position.z = transform.transform.translation.z;
        pose_stamp.pose.orientation = transform.transform.rotation;
        if(child_id != string("base_link")){
            ROS_INFO("[GetTFPose] %s -> %s, pose = (%0.3f, %0.3f, %0.3f).", father_id.c_str(), child_id.c_str(),
                pose_stamp.pose.position.x, pose_stamp.pose.position.y, tf2::getYaw(pose_stamp.pose.orientation));
        }
        return true;  
    }catch (tf2::TransformException &ex) {
        // ROS_WARN("%s", ex.what());
        ros::Duration(0.01).sleep();  
    }
    return false;  
}   

// 获取TF数据
bool TaskManagerNode::GetTFPose(string father_id, string child_id, geometry_msgs::PoseStamped& pose_stamp){
    return GetTFPose(father_id, child_id, pose_stamp, ros::Time(0));
}


/*******************************************
 * 发布报警信息
 *******************************************/
// 生成报警消息
cln_msgs::AlarmMsg TaskManagerNode::GenClnAlarmMsg(int level, long event_code, string event_key){
    cln_msgs::AlarmMsg alarm_msg;
    cln_msgs::AlarmMsg nodocker_alarm;
    alarm_msg.node_name = ros::this_node::getName();
    alarm_msg.event_time = ros::Time::now();
    alarm_msg.event_voice.default_voice_key = "";
    alarm_msg.event_code = event_code;
    alarm_msg.event_level = level;
    int language = ros::param::param<int>("/language", 0);  //0 Chinese,1 English

    // 获取多国语言参数
    string alarm_event_key = "/translation/" + event_key;
    string event_code_str;
    switch(language){
        case 0:
            if(ros::param::has(alarm_event_key + "/cn")){
                ros::param::get(alarm_event_key + "/cn", event_code_str);
            }
            break;
        case 1:
            if(ros::param::has(alarm_event_key + "/en")){
                ros::param::get(alarm_event_key + "/en", event_code_str);
            }
            break;
    }
    alarm_msg.event_code_str = event_code_str;  
    alarm_msg.event_data = event_code_str;
    return alarm_msg;
}

// 发布报警消息
void TaskManagerNode::PubAlarmMsg(int level, string event_key){
    cln_msgs::AlarmMsg alarm_msg = GenClnAlarmMsg(level, 0X0619, event_key);
    __alarm_event_pub.publish(alarm_msg);
}

/*******************************************
 * 在线任务相关话题订阅
 *******************************************/
// 地图名称订阅函数
void TaskManagerNode::MapNameSub(const std_msgs::StringConstPtr& msg){
    __map_name = msg->data;
    LogPub("INFO", "[MapNameSub]map_name=" + __map_name);
    // 地图切换时，自动结束上个地图任务
    string task_echo_id = "";
    TaskEcho task_echo;
    if(__task_echo_ptr){
        task_echo = __msg_convert.V3Msgs2TaskEcho(*__task_echo_ptr);
        task_echo_id = task_echo.task_id;
    }

    if(__last_map_name != __map_name && !__last_map_name.empty()){
        if(!__online_tasks_deque.empty()){
            // 更新状态 
            for(auto& taskgroup_ptr : __online_tasks_deque){
                for(auto& task_single : taskgroup_ptr->task_list){
                    // 只取消上个地图任务，如果地图名称不匹配则跳出
                    if(task_single.task_base.map_base.name != __last_map_name){
                        break;
                    }
                    else{
                        if(task_echo_id == task_single.task_base.task_id){
                            // 添加到缓存队列
                            __task_finished_cache[taskgroup_ptr->group_id] = *taskgroup_ptr;
                            break;
                        }
                        if(task_single.task_echo.task_status == TaskState::UNKNOWN){
                            task_single.task_echo.task_status = TaskState::USER_CANCEL;
                            // 记录到数据库
                            TaskSingleEndHandle(*taskgroup_ptr, task_single.task_base.task_id, false);
                        }                        
                    }                    
                }
            }
            // 取消正在执行的任务
            while(!__online_tasks_deque.empty()){
                auto task_group_ptr = __online_tasks_deque.front();
                // 是否删除当前任务组，根据当前任务组末尾任务判断
                if(!task_group_ptr->task_list.empty()){
                    auto back_task = task_group_ptr->task_list.back();
                    if(back_task.task_base.map_base.name == __last_map_name){
                        // 更新组合任务清洁报告
                        TaskGroupEndHandle(*task_group_ptr, false);
                        LogPub("ERROR", "[MapNameSub] 地图切换, 自动移除组合任务。group_name=" + task_group_ptr->group_name
                            + ", group_id=" + task_group_ptr->group_id);
                        __online_tasks_deque.pop_front();
                    }
                }
                else{
                     break;
                }
            }

            // 结束正在执行的任务
            if(!task_echo_id.empty()){
                TaskSingle curr_task;
                curr_task.task_echo = task_echo;
                curr_task.task_base.task_name = task_echo.task_name;
                curr_task.task_base.task_id = task_echo.task_id;
                CancelTask2NavModule(curr_task);
            }
        }
    }
    // 更新任务队列数据, 并发布数据
    OnlineTaskDequeUpdatePub();
    __last_map_name = __map_name;
}

// 地图信息订阅函数
void TaskManagerNode::MapInfoSub(const v3_msgs::MapInfoConstPtr& msg){
    __map_info_ptr = boost::make_shared<v3_msgs::MapInfo>(*msg);
}

// 任务回传订阅函数
void TaskManagerNode::TaskEchoSub(const v3_msgs::TaskEchoConstPtr& msg){
    __task_echo_ptr = msg;
}

// P2P任务订阅函数
void TaskManagerNode::P2PMoveSub(const v3_msgs::P2PMoveConstPtr& msg){
    // P2P_MOVE
    if(msg->option == msg->P2P_MOVE){
        LogPub("INFO", "[P2PMoveSub] receive a p2p task. send_node_name=" + msg->send_node_name);
        // 创建任务组
        TaskGroup task_group = GenP2PTask(msg->goal);
        // 添加任务组
        PushTaskGroup(task_group);
    }

    // CANCEL_P2P_MOVE
    if(msg->option == msg->CANCEL_P2P_MOVE){
        LogPub("INFO", "[P2PMoveSub] receive a cancel p2p task. send_node_name=" + msg->send_node_name);
        if(!__online_tasks_deque.empty()){
            auto front_taskgroup = __online_tasks_deque.front();
            if(front_taskgroup->group_name == "p2p_move"){
                Status status = CancelTask2NavModule(front_taskgroup->task_list.front());
                // 等待状态反馈
                if(!status.success){
                    LogPub("ERROR", "[P2PMoveSub]Cancel failed! error=" + status.message);
                }
            }
        }
        else{
            LogPub("ERROR", "[P2PMoveSub]__online_tasks_deque is empty!");
        }
    }
}

// 回桩订阅函数
void TaskManagerNode::GoHomeSub(const v3_msgs::GoHomeConstPtr& msg){
    // 开始回桩 GO_HOME
    if(msg->option == msg->GO_HOME){
        LogPub("INFO", "[GoHomeSub] receive a go_home task. send_node_name=" + msg->send_node_name);
        // 创建任务组
        TaskGroup task_group;
        if(msg->use_pose){
            task_group = GenChargingTask(msg->pose);
            PushTaskGroup(task_group);
        }
        else{
            Status status = GenDefaultChargingTask(task_group);
            // 添加任务组
            if(status.success)
                PushTaskGroup(task_group);
        }

    }

    // 取消回桩 CANCEL_GO_HOME
    if(msg->option == msg->CANCEL_GO_HOME){
        LogPub("INFO", "[GoHomeSub] receive a cancel go_home task. send_node_name=" + msg->send_node_name);
        if(!__online_tasks_deque.empty()){
            auto front_taskgroup = __online_tasks_deque.front();
            if(front_taskgroup->group_name == "charging"){
                Status status = CancelTask2NavModule(front_taskgroup->task_list.front());
                // 等待状态反馈
                if(!status.success){
                    LogPub("ERROR", "[GoHomeSub]Cancel failed! error=" + status.message);
                }
            }
        }
        else{
            LogPub("ERROR", "[GoHomeSub]__online_tasks_deque is empty!");
        }
    }

    // 退桩操作 UNDOCKER
    if(msg->option == msg->UNDOCKER){
        LogPub("INFO", "[GoHomeSub] receive a undock task. send_node_name=" + msg->send_node_name);
        // 待补全
    }
}


// 在线任务控制订阅
void TaskManagerNode::OnlineTaskCtrlSub(const v3_msgs::OnlineTaskCtrlConstPtr& msg){
    if(msg->option == msg->TASK_PAUSE){
        LogPub("INFO", "[OnlineTaskCtrlSub] receive TASK_PAUSE. send_node_name=" + msg->send_node_name);
        PauseTask2NavModule();
    } 

    if(msg->option == msg->TASK_RESUME){
        LogPub("INFO", "[OnlineTaskCtrlSub] receive TASK_RESUME. send_node_name=" + msg->send_node_name);
        ResumeTask2NavModule();
    }

    // 取消任务组 TASK_CANCEL
    if(msg->option == msg->TASK_CANCEL){
        LogPub("INFO", "[OnlineTaskCtrlSub] receive TASK_CANCEL. group_id: " +  msg->group_id
            + ", send_node_name=" + msg->send_node_name);
        TaskGroup task_group;
        task_group.group_id = msg->group_id;
        CancelTaskGroup(task_group);
    }

    // 取消所有任务 TASK_CANCEL_ALL
    if(msg->option == msg->TASK_CANCEL_ALL){
        LogPub("INFO", "[OnlineTaskCtrlSub] receive TASK_CANCEL_ALL. send_node_name=" + msg->send_node_name);
        CancelAllTask();
    }

    if(msg->option == msg->TASK_START){
        LogPub("INFO", "[OnlineTaskCtrlSub] receive TASK_START. group_name: " +  msg->group_name
            + ", send_node_name=" + msg->send_node_name);
        TaskGroup task_group;
        if(!__task_db_ptr->GetTaskGroup(msg->group_name, task_group)){
            LogPub("INFO", string("group_name=" + msg->group_name + " 查询失败。"));
            return;
        }
        TaskGroupComplete(task_group);
        PushTaskGroup(task_group);
    }
}

// 位置确认开关订阅函数
void TaskManagerNode::RobotSafeSub(const std_msgs::BoolConstPtr& msg){
    __robot_safe = msg->data;
    LogPub("INFO", "[RobotSafeSub] robot_safe=" + __msg_convert.Bool2Str(__robot_safe));
}

// 硬件状态订阅函数
void TaskManagerNode::HardwareStateSub(const cln_msgs::HardwareStateConstPtr& msg){
    __hardware_state_ptr = msg;
}

// 地图状态订阅函数
void TaskManagerNode::MapStatusSub(const cln_msgs::MapStatusConstPtr& msg){
    __map_status_ptr = msg;
}


// 地图列表订阅
void TaskManagerNode::MapListSub(const v3_msgs::MapListConstPtr& msg){
    __map_list_ptr = msg;
}

/*******************************************
 * 任务控制请求 -> 导航模块
 *******************************************/

// 请求任务控制
Status TaskManagerNode::RequestTaskControl(TaskSingle& task_single, int option){
    Status status;
    v3_msgs::TaskControllerManager client_data;
    try{
        client_data.request.option = option;
        client_data.request.task_single = __msg_convert.TaskSingle2V3Msgs(task_single);
        if(__task_controller_client.call(client_data)){
            // 解析返回数据
            if(!client_data.response.response.success){
                throw(client_data.response.response.message);
            }
        }
        else{
            throw(string("/task_controller_client call failed!"));
        }
    }catch(string& error){
        status.success = false;
        status.message = error;
        LogPub("ERROR", "[RequestTaskControl]error=" + error);
    }  
    return status;
}

// 暂停任务 -> 导航模块
Status TaskManagerNode::PauseTask2NavModule(){
    Status status;
    LogPub("INFO", "[PauseTask2NavModule]......");
    TaskSingle task_single;
    RequestTaskControl(task_single, v3_msgs::TaskControllerManagerRequest::TASK_PAUSE);
    return status;
}

// 恢复任务 -> 导航模块
Status TaskManagerNode::ResumeTask2NavModule(){
    Status status;
    LogPub("INFO", "[ResumeTask2NavModule]......");
    TaskSingle task_single;
    RequestTaskControl(task_single, v3_msgs::TaskControllerManagerRequest::TASK_RESUME);
    return status;
}

// 取消当前任务 -> 导航模块
Status TaskManagerNode::CancelTask2NavModule(TaskSingle& task_single){
    Status status;
    LogPub("INFO", "[CancelTask2NavModule]task_name=" + task_single.task_base.task_name
        + ", task_id=" + task_single.task_base.task_id);
    RequestTaskControl(task_single, v3_msgs::TaskControllerManagerRequest::USER_CANCEL);
    return status;
}

// 开始执行任务 -> 导航模块
Status TaskManagerNode::StartTask2NavModule(TaskSingle& task_single){
    Status status;
    LogPub("INFO", "[StartTask2NavModule]task_name=" + task_single.task_base.task_name
        + ", task_id=" + task_single.task_base.task_id
        + ", task_type: " + __msg_convert.TaskType2Str(task_single.task_base.task_type)
        );
    RequestTaskControl(task_single, v3_msgs::TaskControllerManagerRequest::TASK_START);
    return status;
}

// 终止任务 -> 导航模块
Status TaskManagerNode::TerminatedTask2NavModule(TaskSingle& task_single){
    Status status;
    LogPub("INFO", "[TerminatedTask2NavModule]task_name=" + task_single.task_base.task_name
        + ", task_id=" + task_single.task_base.task_id);
    RequestTaskControl(task_single, v3_msgs::TaskControllerManagerRequest::TASK_TERMINATED);
    return status;
}


/*******************************************
 * 查询地图基础信息
 *******************************************/
// 查询地图信息
void TaskManagerNode::QueryMapBaseInfo(string map_name, v3_msgs::MapBase& map_base){
    v3_msgs::MapInfoRequest client_data;
    try{
        client_data.request.option = client_data.request.QUERY_MAP_BASE_INFO;
        client_data.request.map_info.base_info.name = map_name;
        if(!__map_info_client.call(client_data)){
            throw(string("report_record_client call failed!"));
        }
        if(!client_data.response.response.success){
            throw(client_data.response.response.message);
        }
        map_base = client_data.response.map_info.base_info;
    }
    catch(string& error){
        LogPub("ERROR", "[QueryMapBaseInfo] map_name = " + map_name
            + ", error=" + error);
    }    
    catch(std::exception& e){
        LogPub("ERROR", "[QueryMapBaseInfo] map_name = " + map_name
            + ", exception=" + std::string(e.what()));
    }
}

/*******************************************
 * 在线任务相关服务
 *******************************************/
// 任务完成记录服务函数 正常或非正常结束当前任务
bool TaskManagerNode::TaskFinishedRecordServer(v3_msgs::TaskFinishedRecordRequest &req, v3_msgs::TaskFinishedRecordResponse &res){
    LogPub("INFO", "[TaskFinishedRecordServer]task_name=" + req.task_single.task_base.task_name
        + ", task_id=" + req.task_single.task_base.task_id);

    try{
        TaskSingle task_single_finished = __msg_convert.V3Msgs2TaskSingle(req.task_single);
        TaskFinishedHandle(task_single_finished);
    }
    catch(string& error){
        LogPub("ERROR", "[TaskFinishedRecordServer]error=" + error);
        res.response.success = false;
        res.response.message = error;
    }    
    catch(std::exception& e){
        LogPub("ERROR", "[TaskFinishedRecordServer] exception=" + std::string(e.what()));
        res.response.success = false;
        res.response.message = std::string(e.what());
    }
    res.response.success = true;
    res.response.message = "ok";
    return true;
}


/*******************************************
 * 在线任务状态发布
 *******************************************/
// 更新任务队列数据, 并发布数据
void TaskManagerNode::OnlineTaskDequeUpdatePub(){
    LogPub("INFO", "[OnlineTaskDequeUpdatePub] deque_size=" + to_string(__online_tasks_deque.size()));
    v3_msgs::OnlineTask onlinetask_msg;
    for(auto task_group_ptr : __online_tasks_deque){
        // http://bug.aotbot.com/bug-view-3114.html ft.wang 20240901 任务清单需要显示，屏蔽此部分内容
        // // 任务清单跳过充电任务和P2P任务 
        // bool is_skip = false;
        // for(auto& task_single : task_group->task_list){
        //     if(IsP2PTask(task_single.task_base.task_type) || 
        //         IsChargingTask(task_single.task_base.task_type)){
        //         is_skip = true;
        //         break;
        //     }
        // }
        // if(is_skip) continue;

        // 任务路径不发布, 避免数据量过大
        TaskGroup task_group = *task_group_ptr;
        for(auto& task_single : task_group.task_list){
            task_single.task_base.paths.clear();
            task_single.task_base.edge_paths.clear();
        }
        onlinetask_msg.taskgroup_list.emplace_back(__msg_convert.TaskGroup2V3Msgs(task_group));
    }
    __online_tasks_deque_pub.publish(onlinetask_msg);
}

// 在线任务状态发布
void TaskManagerNode::OnlineTaskPublish(TaskEcho& task_echo){
    
    if(!__online_tasks_deque.empty()){
        TaskGroup front_group = *(__online_tasks_deque.front());
        for(int i=0; i<front_group.task_list.size(); ++i){
            TaskSingle& curr_task = front_group.task_list[i];
            // 任务路径不发布, 避免数据量过大
            curr_task.task_base.paths.clear();
            curr_task.task_base.edge_paths.clear();

            if(curr_task.task_base.task_id == task_echo.task_id){
                // 更新任务状态数据  // 此处存在BUG，待太平修复 终止任务,返回服务后会多发一帧EXECUING回传状态
                curr_task.task_echo = task_echo;

                // 同步更新__online_tasks_deque中状态数据
                __online_tasks_deque.front()->task_list[i].task_echo = task_echo;
                if(!__online_tasks_deque.front()->task_list[i].task_base.is_assigned){
                    __online_tasks_deque.front()->task_list[i].task_base.is_assigned = true;
                }

                __curr_task_single = boost::make_shared<TaskSingle>(curr_task);
                v3_msgs::TaskSingle task_single_v3 = __msg_convert.TaskSingle2V3Msgs(curr_task);
                task_single_v3.stamp = ros::Time::now();
                __online_task_single_pub.publish(task_single_v3);

                // // 打印进度
                // LogPub("INFO", "[single: " + to_string(i+1) + "/" + to_string(front_group.task_list.size())
                //     + " | group: 1/" + to_string(__online_tasks_deque.size()) + "]"
                //     + ", task_name: " + curr_task.task_base.task_name
                //     + ", task_id: " + curr_task.task_base.task_id
                //     + ", process: " + to_string(curr_task.task_echo.progress)
                //     + ", task_status: " + __msg_convert.TaskState2Str(curr_task.task_echo.task_status)
                // );
                break;
            }
        }
        v3_msgs::TaskGroup task_group_v3 = __msg_convert.TaskGroup2V3Msgs(front_group);
        task_group_v3.stamp = ros::Time::now();
        __online_task_group_pub.publish(task_group_v3); 
    }
    else{
        // 用于任务队列为空，但任务状态仍需发布，如暂停/恢复状态
        TaskSingle task_single;
        task_single.task_echo = task_echo;
        task_single.task_echo.task_name = task_echo.task_id.empty() ? string("") : string("unknown");
        v3_msgs::TaskSingle task_single_v3 = __msg_convert.TaskSingle2V3Msgs(task_single);
        task_single_v3.stamp = ros::Time::now();
        __online_task_single_pub.publish(task_single_v3);
    }
    
}

// 在线任务状态发布 空闲状态
void TaskManagerNode::OnlineTaskPublishIdle(){
    __curr_task_single = nullptr;
    v3_msgs::TaskSingle task_single_v3;
    task_single_v3.stamp = ros::Time::now();
    __online_task_single_pub.publish(task_single_v3);
    v3_msgs::TaskGroup task_group_v3;
    task_group_v3.stamp = ros::Time::now();
    __online_task_group_pub.publish(task_group_v3);    
}

// // 通知上传云平台
// void TaskManagerNode::UploadGroup(string group_id){
//     std_msgs::String msg;
//     msg.data = group_id;
//     __notification_record_upload.publish(msg);
// }

/*******************************************
 * 任务状态监控
 *******************************************/
// 检测当前状态，任务是否可以正常下发
bool TaskManagerNode::CheckStatusIsValid(std::shared_ptr<TaskSingle> task_single_ptr){
    // 检测任务状态
    if(__robot_lock_state){     //锁机状态
        cout << "[CheckStatusIsValid] robot_lock_state: " << __robot_lock_state << endl;
        return false;
    }
    if(!__robot_safe){  //位置确认开关状态
        cout << "[CheckStatusIsValid] robot_safe: " << __robot_safe << endl;
        return false;
    }      
    if(__map_status_ptr && (__map_status_ptr->in_creating || __map_status_ptr->in_updating)){
        cout << "[CheckStatusIsValid] in_creating: " << __map_status_ptr->in_creating
            << ", in_updating: " << __map_status_ptr->in_updating << endl;
        return false;       //建图或更新中
    }        
    if(__hardware_state_ptr && (int(__hardware_state_ptr->bat_state) == 1)){
        cout << "[CheckStatusIsValid] bat_state: " << int(__hardware_state_ptr->bat_state) << endl;
        return false;       //充电中
    }
    // 新增电量小于20时不下发除回桩和P2P以外任务
    if(__hardware_state_ptr && (int(__hardware_state_ptr->bat_percentage) < __power_gohome)){
        if(task_single_ptr && !IsP2PTask(task_single_ptr->task_base.task_type) 
            && !IsChargingTask(task_single_ptr->task_base.task_type)){
            return false;       //电量小于20
        }
    }
    return true;
}

// 任务组结束，自动回桩或回停靠点处理
void TaskManagerNode::TaskEmptyAndGoHome(){
    if(!__hardware_state_ptr){
        LogPub("ERROR", "[TaskEmptyAndGoHome] __hardware_state_ptr is nullptr");
        return;
    }
    if(__hardware_state_ptr->manual){
        LogPub("INFO", "[TaskEmptyAndGoHome] manual mode, no auto handle");
        return;
    }
        
    // 判断参数配置
    TaskGroup task_group;
    Status status;
    __all_finished_doing = ros::param::param<int>("/cleantask/all_finished", 1);   // 所有工作结束选项,0 - 停在原地,1 - 回充电桩,3 - 回停靠点
    switch(__all_finished_doing){
        case 0:             // 停在原地
            ;
            break;
        case 1:             // 回充电桩
            {
                // 默认充电桩回桩任务
                status = GenDefaultChargingTask(task_group);
                if(status.success){
                    PushTaskGroup(task_group);
                }
                else{
                    //前往最近的充电桩
                    status = GenNearestChargingTask(task_group);
                    if(status.success){
                        PushTaskGroup(task_group);
                    }
                    else{
                        // 默认回停靠点任务
                        status = GenDefaultStopTask(task_group);
                        if(status.success){
                            // 发布报警信息 LEVEL4等级报警（未配置充电桩，将前往停靠点）
                            PubAlarmMsg(cln_msgs::AlarmMsg::LEVEL_4, "go_default_stop_point");
                            PushTaskGroup(task_group);
                        }
                        else{
                            //前往最近的停靠点
                            status = GenNearestStopTask(task_group);
                            if(status.success){
                                // LEVEL4等级报警（未配置充电桩，将前往停靠点）
                                PubAlarmMsg(cln_msgs::AlarmMsg::LEVEL_4, "go_default_stop_point");
                                PushTaskGroup(task_group);
                            }  
                            else{
                                // LEVEL4等级报警（未配置充电桩，回桩失败）
                                PubAlarmMsg(cln_msgs::AlarmMsg::LEVEL_4, "go_home_failed");
                            }                   
                        }
                    }
                }
            }
            break;
        case 2:             // ;
            break;
        case 3:             // 回停靠点
            {  
                // 默认回停靠点任务
                status = GenDefaultStopTask(task_group);
                if(status.success){
                    PushTaskGroup(task_group);
                }
                else{
                    //前往最近的停靠点
                    status = GenNearestStopTask(task_group);
                    if(status.success){
                        PushTaskGroup(task_group);
                    }                     
                }
            }
            break;
        default:
            LogPub("ERROR", "[TaskEmptyAndGoHome] all_finished_doing error! value = " + to_string(__all_finished_doing));
    }
}

// 任务队列插入
void TaskManagerNode::PushTaskGroup(TaskGroup task_group){
    LogPub("INFO", "准备追加组合任务到队列, group_name=" + task_group.group_name
        + ", group_id=" + task_group.group_id
        + ", timertask_name=" + task_group.timertask_name
        + ", timertask_id=" + task_group.timertask_id
        + ", deque_size=" + to_string(__online_tasks_deque.size()));
    auto task_group_ptr = std::make_shared<TaskGroup>(task_group);
    // 任务数据校验
    if(task_group_ptr->group_name.empty()){
        LogPub("ERROR", "[PushTaskGroup] group_name is empty!");
        return;
    }
    if(task_group_ptr->group_id.empty()){
        task_group_ptr->group_id = GenerateId();
    }
    // 任务组初始数据赋值
    ros::Time now = ros::Time::now();
    for(auto& task_single : task_group_ptr->task_list){
        string map_name = task_single.task_base.map_base.name;
        string task_name = task_single.task_base.task_name;
        // 补全所有子任务信息
        if(task_name != "charging" && task_name != "p2p_move" && task_name != "trans_floor"){
            // 检测单个任务是否存在
            if(!__task_db_ptr->CheckTaskSingleExist(map_name, task_name)){
                task_single.task_echo.task_status = TaskState::ERROR;
                task_single.task_echo.error_code.error_msg = "task_name is not exist!";
            }
            else{
                 __task_db_ptr->GetTaskSingle(map_name, task_name, task_single);
                // 检验任务面积
                auto task_type = task_single.task_base.task_type;       //是否示教路线模式
                auto control_mode = task_single.task_base.control_mode;
                bool is_teach_route = (task_type == TaskType::TEACH_TASK) && 
                    (control_mode == TaskControlMode::SWEEP_EDGE_MODE || control_mode == TaskControlMode::FOLLOW_MODE);
                if(task_single.task_base.task_area == 0.0 && !is_teach_route){
                    // 重新生成总清扫面积
                    // Status gen_status = __path_gen_interface.GenPolygonTaskWithClnMsg(task_single, true);
                    Status gen_status = __path_gen_interface.GenPolygonTaskWithV3Msg(task_single, true);
                    if(!gen_status.success){
                        LogPub("WARN", "重新生成总清扫面积异常, error=" + gen_status.message);
                    }
                    else{
                        LogPub("INFO", "重新生成总清扫面积 task_area: " + to_string(task_single.task_base.task_area));
                        // 数据更新保存
                        __task_db_ptr->UpdateTaskArea(map_name, task_name, task_single.task_base.task_area);
                    }
                }
                // 补全地图基础数据
                QueryMapBaseInfo(map_name, task_single.task_base.map_base);
            }
        }   
        // 所有子任务group_name,group_id必须一致
        task_single.task_base.group_name = task_group_ptr->group_name;
        task_single.task_base.group_id = task_group_ptr->group_id;
        // 修复兼容任务已存在task_id，导致重复下发问题
        // if(task_single.task_base.task_id.empty()){
        //     task_single.task_base.task_id = GenerateId();
        // }
        task_single.task_base.task_id = GenerateId();
        task_single.task_echo.add_time = __msg_convert.TimestampFormat(now.sec);
        // 状态重置
        if(task_single.task_echo.task_status == TaskState::ERROR)
            continue;
        task_single.task_echo.task_status = TaskState::UNKNOWN;
        LogPub("INFO", "添加任务, map_name=" + map_name 
            + ", task_name=" + task_name 
            + ", task_id=" + task_single.task_base.task_id
            + ", group_id=" + task_single.task_base.group_id
            + ", task_area=" + to_string(task_single.task_base.task_area)
            + ", map_width=" + to_string(task_single.task_base.map_base.metadata.width)
            + ", map_height=" + to_string(task_single.task_base.map_base.metadata.height)
            );
    }

    // 待补全 任务队列长度限制

    // 任务队列插入管理
    if(__online_tasks_deque.empty()){
        // 任务队列为空，直接插入
        __online_tasks_deque.emplace_back(task_group_ptr);      //将对象直接追加到末尾
    }
    else{   // 优先处理回桩任务和P2P任务
        // 任务队列不为空，判断当前任务组是否与队首任务组相同
        auto front_taskgroup = __online_tasks_deque.front();
        // -------------------------charging-----------------------------
        if(front_taskgroup->group_name == "charging"){
            // 首个任务是充电任务
            LogPub("WARN", "[PushTaskGroup][front_taskgroup] charging, group_id=" + front_taskgroup->group_id);
            if(task_group_ptr->group_name == "charging" || task_group_ptr->group_name == "p2p_move"){
                return;
            }
            else{   //追加到末尾
                __online_tasks_deque.emplace_back(task_group_ptr);      //将对象直接追加到末尾
            }
        }
        // -------------------------p2p_move-----------------------------
        else if(front_taskgroup->group_name == "p2p_move"){
            // 首个任务是P2P任务
            LogPub("WARN", "[PushTaskGroup][front_taskgroup] p2p_move, group_id=" + front_taskgroup->group_id);
            if(task_group_ptr->group_name == "charging" || task_group_ptr->group_name == "p2p_move"){
                // 终止正在执行的P2P，开始回桩任务/新P2P任务
                Status status = CancelTask2NavModule(front_taskgroup->task_list.front());
                // 等待状态反馈
                if(status.success){
                    // 插入充电任务
                    __online_tasks_deque.emplace_front(task_group_ptr);   // 添加到队头
                }
                else{
                    LogPub("ERROR", "[PushTaskGroup]Push failed! error=" + status.message);
                }
            }
            else{   
                __online_tasks_deque.emplace_back(task_group_ptr);      //将对象直接追加到末尾
            }
        }
        // -------------------------other-----------------------------
        else{
            // 首个任务是其它类型任务
            if(task_group_ptr->group_name == "charging" || task_group_ptr->group_name == "p2p_move"){
                // 终止正在执行的P2P，开始回桩任务/新P2P任务
                Status status = TerminatedTask2NavModule(front_taskgroup->task_list.front());
                // 等待状态反馈
                if(status.success){
                    // 插入充电任务
                    __online_tasks_deque.emplace_front(task_group_ptr);   // 添加到队头
                }
                else{
                    LogPub("ERROR", "[PushTaskGroup]Push failed! error=" + status.message);
                }
            }
            else{   
                __online_tasks_deque.emplace_back(task_group_ptr);      //将对象直接追加到末尾
            }
        }
    }

    // 更新任务队列数据, 并发布数据
    OnlineTaskDequeUpdatePub();

    LogPub("INFO", "追加结束, group_name=" + task_group_ptr->group_name
        + ", group_id=" + task_group_ptr->group_id
        + ", timertask_name=" + task_group_ptr->timertask_name
        + ", timertask_id=" + task_group_ptr->timertask_id
        + ", deque_size=" + to_string(__online_tasks_deque.size()));
}



// 发布语音
void TaskManagerNode::PublishWorkVoice(){
    cln_msgs::VoiceOutput voice_msg;
    voice_msg.default_voice_key = "work_data";
    __voice_pub.publish(voice_msg);
}


// 任务状态监控函数
void TaskManagerNode::TasksMonitor(){
    ros::Rate rate(50);    
    pair<bool, ros::Time> task_finished_go_home = {false, ros::Time(0)};    //在线任务队列结束回桩判断

    // 任务记录自动清理
    ros::Time last_taskrecord_autoclean_time = {ros::Time(0)};  
    int taskrecord_max_days = 30; 

    string curr_task_id;
    ros::Time last_time = ros::Time::now();
    TaskType last_task_type = TaskType::UNKNOWN;             //最后一次下发的任务类型
    while(!ros::isShuttingDown()){
        ros::spinOnce();
        rate.sleep();

        // 时间间隔控制，防止其它线程阻塞
        ros::Time now_time = ros::Time::now();
        // 任务记录数据库自动清理  30分钟检测一次
        if((now_time - last_taskrecord_autoclean_time).toSec() > 30*60){
            last_taskrecord_autoclean_time = now_time;
            if(__task_db_ptr){
                __task_db_ptr->TaskRecordAutoClean(taskrecord_max_days);
                LogPub("INFO", "任务记录数据库自动清理");
            }
        }

        if((now_time - last_time).toSec() > 0.5){           // 2Hz
            last_time = now_time;
        }
        else continue;

        // 实时任务状态获取
        if(!__task_echo_ptr) continue;
        TaskEcho task_echo = __msg_convert.V3Msgs2TaskEcho(*__task_echo_ptr);
        // ROS_INFO_STREAM("task_name=" << task_echo.task_name 
        //     << ", task_id: " << task_echo.task_id
        //     << ", task_status: " << __msg_convert.TaskState2Str(task_echo.task_status)
        // );


        // 状态逻辑判断
        switch(task_echo.task_status){
            case TaskState::IDLE:
                {
                    if(__online_tasks_deque.empty()){ 
                        // 在线任务状态发布 空闲状态
                        OnlineTaskPublishIdle();
                    }
                    else{
                        auto task_group_ptr = __online_tasks_deque.front();
                        // 获取当前需要执行任务
                        bool is_first = false;
                        std::shared_ptr<TaskSingle> task_single_ptr = FindOnlineTaskSingleExec(is_first);
                        // 判断当前状态, 是否满足下发条件
                        if(!task_single_ptr){
                            cout << "task_single_ptr is nullptr!" << endl;
                            break;
                        }
                        if(!CheckStatusIsValid(task_single_ptr)){
                            cout << "CheckStatusIsValid failed!" << endl;
                            // 在线任务状态发布 空闲状态
                            OnlineTaskPublishIdle();    
                            break;
                        }
                        cout << "is_first: " << is_first << ", curr_task_id: " << curr_task_id << endl;
                        
                        // 任务下发, 只下发一次
                        if(curr_task_id != task_single_ptr->task_base.task_id){
                            curr_task_id = task_single_ptr->task_base.task_id;
                            // 开始执行任务
                            StartTask2NavModule(*task_single_ptr);
                            last_task_type = task_single_ptr->task_base.task_type;
                            // 通知任务报告服务记录状态
                            vector<TaskRecord> task_records;
                            if(is_first){     //任务组开始
                                // 注意，此处调用可能存在生成耗时情况，后续优化
                                TaskGroupRecord(*task_group_ptr, true, task_records);
                            }
                            TaskSingleRecord(*task_group_ptr, true, task_single_ptr->task_base.task_id, task_records);

                            // 最长等待5秒状态刷新, 超时需要进入异常处理机制，待补全
                            for(int i=0; i<50; ++i){
                                ros::spinOnce();
                                ros::Duration(0.1).sleep();
                                if(__task_echo_ptr->task_id == curr_task_id){
                                    break;
                                }
                            }
                        }

                    }
                }
                break;
            case TaskState::EXECUING:
                {
                    OnlineTaskPublish(task_echo);       // 在线任务状态发布
                }
                break;  
            case TaskState::PAUSE:  
                {
                    OnlineTaskPublish(task_echo);       // 在线任务状态发布
                }
                break;  
            case TaskState::BLOCKED:  
                {
                    OnlineTaskPublish(task_echo);       // 在线任务状态发布
                }
                break;
            case TaskState::ERROR:  
                {
                    OnlineTaskPublish(task_echo);       // 在线任务状态发布
                
                }
                break;            
            default:
                break;
        }


        // 自动结束任务状态判断
        if(!__online_tasks_deque.empty()){ 
            task_finished_go_home.first = false;
            task_finished_go_home.second = ros::Time::now();
        }
        else{
            if(!task_finished_go_home.second.isZero()){     //非开机后默认队列为空状态
                // 自动模式下，机器人检测到无任务，5S后自动回桩
                float time_interval = (ros::Time::now() - task_finished_go_home.second).toSec();
                // cout << "[TasksMonitor] time_interval: " << time_interval << endl;
                if(time_interval > 5.0){
                    // 自动回桩
                    if(!task_finished_go_home.first 
                        && last_task_type != TaskType::CHARGING_TASK && last_task_type != TaskType::P2P_TASK){
                        task_finished_go_home.first = true;
                        // 开始下发自动回桩
                        LogPub("INFO", "[TaskEmptyAndGoHome] 5s no task, ready go home! last_task_type: "
                            + __msg_convert.TaskType2Str(last_task_type));
                        TaskEmptyAndGoHome();
                    }
                }
                if(task_finished_go_home.first){
                    task_finished_go_home.second = ros::Time::now();
                }
            }
        }

        // 工作语音定频发布
        if(__work_voice.enable){
            if(task_echo.task_status == TaskState::EXECUING){
                float time_interval = (ros::Time::now() - __work_voice.last_pub_time).toSec();
                if(time_interval > __work_voice.frequency){
                    // 非充电中、非P2P任务才播报语音
                    if(__curr_task_single){
                        // 检测当前任务是否充电或P2P任务
                        if(!IsP2PTask(__curr_task_single->task_base.task_type) 
                            && !IsChargingTask(__curr_task_single->task_base.task_type)){
                            // 充电中或急停中，不播报语音
                            if(__hardware_state_ptr && (int(__hardware_state_ptr->bat_state) == 0) && (int(__hardware_state_ptr->e_stop) == 0)){
                                ROS_INFO("发布工作语音......");
                                PublishWorkVoice();
                                __work_voice.last_pub_time = ros::Time::now();
                            }
                        }
                    }

                }
            }
            else{
                __work_voice.last_pub_time = ros::Time::now();
            }
        } 
    }
}



/*******************************************
 * 任务取消管理
 *******************************************/
// 取消正在执行的首个组合任务
void TaskManagerNode::CancelFrontTaskGroup(){

    // 获取当前正在执行的任务ID
    string task_echo_id = "";
    if(__task_echo_ptr){
        TaskEcho task_echo = __msg_convert.V3Msgs2TaskEcho(*__task_echo_ptr);
        task_echo_id = task_echo.task_id;
    }

    if(!__online_tasks_deque.empty()){   
        auto& front_taskgroup = __online_tasks_deque.front();
        cout << "[CancelFrontTaskGroup] group_id: " << front_taskgroup->group_id << endl;
        // 所有未下发任务置为取消状态
        for(int i=0; i<front_taskgroup->task_list.size(); ++i){
            auto& task_single = front_taskgroup->task_list[i];
            // 检测当前正在执行的单个任务
            if(task_echo_id == task_single.task_base.task_id){
                // 添加到缓存队列
                __task_finished_cache[front_taskgroup->group_id] = *front_taskgroup;
            }
            if(task_single.task_echo.task_status == TaskState::UNKNOWN){
                task_single.task_echo.task_status = TaskState::USER_CANCEL;
            }
        }    
    }

    if(!task_echo_id.empty()){
        // 取消正在执行任务，也可解决异常问题，如任务队列为空，但task_echo仍回传数据问题
        TaskSingle task_single;
        task_single.task_base.task_id = task_echo_id;
        // 取消任务执行
        Status status = CancelTask2NavModule(task_single);
        if(!status.success){
            LogPub("ERROR", "[CancelTaskGroup]Cancel failed! error=" + status.message);
        }  
    }
    else{
        // 异常情况处理，修复如位置确认开关关闭等条件，导致任务在任务清单，但未下发到导航的取消失败问题
        if(!__online_tasks_deque.empty()){   
            auto& front_taskgroup = __online_tasks_deque.front();
            // 删除此组合任务
            DeleteTaskGroup(front_taskgroup->group_id);
        }
    }
}


// 取消指定组合任务 
void TaskManagerNode::CancelTaskGroup(TaskGroup& taskgroup){
    cout << "[CancelTaskGroup] group_id: " << taskgroup.group_id << endl;
    if(__online_tasks_deque.empty()){
        return;
    }
    // 检测是否为当前正在执行的组合任务
    auto& front_taskgroup = __online_tasks_deque.front();
    bool is_curr_taskgroup = taskgroup.group_id == front_taskgroup->group_id;
    cout << "[CancelTaskGroup] is_curr_taskgroup: " << is_curr_taskgroup << endl;
    if(is_curr_taskgroup){  // 取消当前正在执行的组合任务
        CancelFrontTaskGroup();
    }
    else{   // 取消非正在执行的组合任务
        for(auto& task_single : taskgroup.task_list){
            // 存在任务插入队列时，查询失败情况
            if(task_single.task_echo.task_status != TaskState::ERROR){
                task_single.task_echo.task_status = TaskState::USER_CANCEL;
            }
            // 记录到数据库
            TaskSingleEndHandle(taskgroup, task_single.task_base.task_id, false);
        }  
        // 更新组合任务清洁报告
        TaskGroupEndHandle(taskgroup, false);
        // 删除此组合任务
        DeleteTaskGroup(taskgroup.group_id);
    }

    cout << "[CancelTaskGroup] OnlineTaskDequeUpdatePub..."  << endl;
    // 更新任务队列数据, 并发布数据
    OnlineTaskDequeUpdatePub();    
}

// 取消所有任务
void TaskManagerNode::CancelAllTask(){
    ros::Time now = ros::Time::now();

    // 结束首个任务
    CancelFrontTaskGroup();

    if(__online_tasks_deque.empty()){
        // 更新任务队列数据, 并发布数据
        OnlineTaskDequeUpdatePub();
        return;
    }

    // 更新除首个任务之外的所有任务记录
    string front_group_id = __online_tasks_deque.front()->group_id;
    for(auto& taskgroup_ptr : __online_tasks_deque){
        cout << "[CancelAllTask] group_id: " << taskgroup_ptr->group_id << endl;
        if(taskgroup_ptr->group_id != front_group_id){
            // 依次记录单个任务数据
            for(auto& task_single : taskgroup_ptr->task_list){
                // 存在任务插入队列时，查询失败情况
                if(task_single.task_echo.task_status != TaskState::ERROR){
                    task_single.task_echo.task_status = TaskState::USER_CANCEL;
                }
                // 记录到数据库
                TaskSingleEndHandle(*taskgroup_ptr, task_single.task_base.task_id, false);
            }      

            // 记录组合任务
            TaskGroupEndHandle(*taskgroup_ptr, false);
        }
    }

    // 清空任务队列, 并发布数据
    __online_tasks_deque.clear();
    OnlineTaskDequeUpdatePub();
}



/*******************************************
 * 任务结束管理
 *******************************************/
// 获取当前最新任务组数据
void TaskManagerNode::GetCurrTaskGroup(string group_id, TaskGroup& task_group){
    // 查找所属任务组
    if(__online_tasks_deque.empty() && __task_finished_cache.empty()){
        LogPub("ERROR", "[TaskFinishedHandle]Can not find task group! group_id=" + group_id);
        return;
    }
    bool is_in_cache = __task_finished_cache.count(group_id) != 0;
    cout << "[debug] [UpdateTaskEcho] is_in_cache is " << is_in_cache << endl;
    if(!__online_tasks_deque.empty() && group_id == __online_tasks_deque.front()->group_id){
        task_group = *(__online_tasks_deque.front());
    }
    else{
        if(!is_in_cache){
            LogPub("ERROR", "[TaskFinishedHandle]Can not find task group! group_id=" + group_id);
        }
        task_group = __task_finished_cache[group_id];
    } 
    return;   
}

// 删除已完成的任务组
void TaskManagerNode::RemoveFinishedTaskGroup(string group_id){
    // 删除已完成的任务组
    if(__task_finished_cache.count(group_id) != 0){
        __task_finished_cache.erase(group_id);
    }
    if(!__online_tasks_deque.empty() && (group_id == __online_tasks_deque.front()->group_id)){
        __online_tasks_deque.pop_front();
        LogPub("INFO", "移除组合任务, group_id: " + group_id + ", deque_size=" + to_string(__online_tasks_deque.size()));
    }
}

// 更新单个任务回传状态数据
void TaskManagerNode::UpdateTaskEcho(string group_id, TaskSingle& task_single){
    // 查找所属任务组
    bool is_in_cache = __task_finished_cache.count(group_id) != 0;
    cout << "[debug] [UpdateTaskEcho] is_in_cache is " << is_in_cache << endl;
    bool find_task_group = false;
    if(!__online_tasks_deque.empty()){
        for(auto task_group_ptr : __online_tasks_deque){
            if(task_group_ptr->group_id == group_id){
                // 更新单个任务结束数据
                for(int i=0; i<task_group_ptr->task_list.size(); ++i){
                    if(task_group_ptr->task_list[i].task_base.task_id == task_single.task_echo.task_id){
                        // 更新任务状态数据
                        task_group_ptr->task_list[i].task_echo = task_single.task_echo;
                        find_task_group = true;
                        break;
                    }
                }
            }
        }   
    }
    else{
        if(__task_finished_cache.count(group_id) != 0){
            find_task_group = true;
            // 更新单个任务结束数据
            for(int i=0; i<__task_finished_cache[group_id].task_list.size(); ++i){
                if(__task_finished_cache[group_id].task_list[i].task_base.task_id == task_single.task_echo.task_id){
                    // 更新任务状态数据
                    __task_finished_cache[group_id].task_list[i].task_echo = task_single.task_echo;
                    break;
                }
            }
        }

    } 
    if(!find_task_group){
        LogPub("ERROR", "[UpdateTaskEcho]Can not find task group! group_id=" + group_id);
    }
    return;     
}


// 任务结束管理
void TaskManagerNode::TaskFinishedHandle(TaskSingle task_single_finished){
    string group_id = task_single_finished.task_base.group_id;
    string task_id = task_single_finished.task_echo.task_id;

    LogPub("INFO", "任务结束处理, group_name: " + task_single_finished.task_base.group_name 
        + ", group_id: " + group_id 
        + ", task_name: " +  task_single_finished.task_base.task_name 
        + ", task_id: " +  task_id
        + ", task_status: " + __msg_convert.TaskState2Str(task_single_finished.task_echo.task_status));


    // 终止状态处理,不记录数据到数据库，等待下次下发处理
    if(task_single_finished.task_echo.task_status == TaskState::TERMINATED){
        // 更新任务状态
        UpdateTaskEcho(group_id, task_single_finished);     //同步回传的单个任务数据
        OnlineTaskPublish(task_single_finished.task_echo);  // 在线任务状态发布
        return;
    }

    TaskGroup task_group;
    UpdateTaskEcho(group_id, task_single_finished);     //同步回传的单个任务数据
    GetCurrTaskGroup(group_id, task_group);
    if(task_group.group_id.empty()){
        cout << "[debug] [TaskFinishedHandle] get task_group failed!" << endl;
        return;
    }

    cout << "[debug] [TaskFinishedHandle]......005 timertask_name: " << task_group.timertask_name
        << ", timertask_id: " << task_group.timertask_id
        << endl;
    

    // 更新单个任务清洁报告
    for(int i=0; i<task_group.task_list.size(); ++i){   
        if(task_group.task_list[i].task_base.task_id == task_id){
            // // 更新任务状态
            // task_group.task_list[i].task_echo = task_single_finished.task_echo;
            LogPub("INFO", "准备更新任务记录数据..., task_id: " + task_id
                + ", 进展: [" + to_string(i+1) + "/" + to_string(task_group.task_list.size()) + "]"
                + ", task_area: " + to_string(task_group.task_list[i].task_base.task_area)
                );
            // 单个任务清洁报告
            TaskSingleEndHandle(task_group, task_id, true);
            break;
        }     
    }

    cout << "[debug] [TaskFinishedHandle] IsFronTaskGroupFinished..." << endl;
    // 如果任务组结束，通知任务组记录保存
    if(IsFronTaskGroupFinished()){
        GetCurrTaskGroup(group_id, task_group);     //获取单个任务同步清洁报告的最新任务组数据
        if(task_group.group_id.empty()){
            cout << "[debug] [TaskFinishedHandle] get task_group failed!" << endl;
            return;
        }
        // 打印任务数据
        for(int i=0; i<task_group.task_list.size(); ++i){ 
            cout << "[debug] [TaskFinishedHandle] i: " << i
                << ", task_name: " << task_group.task_list[i].task_base.task_name
                << ", task_id: " << task_group.task_list[i].task_base.task_id
                << ", task_area: " + to_string(task_group.task_list[i].task_base.task_area)
                << ", coverage_rate: " + to_string(task_group.task_list[i].task_echo.coverage_rate)
                << ", coverage_rate: " + to_string(task_group.task_list[i].task_echo.coverage_rate)
                << endl;
        }

        // 记录到数据库
        TaskGroupEndHandle(task_group, true);
        // 删除已完成的任务组
        RemoveFinishedTaskGroup(group_id);

    }

    // 更新任务队列数据, 并发布数据
    OnlineTaskDequeUpdatePub();
}



// 单个任务结束处理
void TaskManagerNode::TaskSingleEndHandle(TaskGroup task_group, string task_id, bool need_record){
    LogPub("INFO", "单个任务结束，准备更新单个任务记录数据..., task_id: " + task_id);
    TaskSingle task_single;
    for(int i=0; i<task_group.task_list.size(); ++i){        
        if(task_group.task_list[i].task_base.task_id == task_id){
            task_single = task_group.task_list[i];
            break;
        }
    }
    if(task_single.task_base.task_id.empty()) return;

    if(need_record){
        vector<TaskRecord> task_records;
        TaskSingleRecord(task_group, false, task_id, task_records);
        cout << "[debug] [TaskSingleEndHandle] task_records size: " << task_records.size() << endl;
        if(task_records.empty()){
            LogPub("ERROR", "[TaskSingleEndHandle]Can not find task record! group_id=" + task_group.group_id);

        }
        else{
            TaskGroup task_group_record = task_records.front().task_group;
            for(auto& task_single_item : task_group_record.task_list){
                // 校验task_id
                if(task_single_item.task_base.task_id == task_id){
                    // 更新清洁报告相关数据
                    task_single.task_base.estimated_distance = task_single_item.task_base.estimated_distance;
                    task_single.task_base.estimated_speed = task_single_item.task_base.estimated_speed;
                    task_single.task_base.estimated_efficiency = task_single_item.task_base.estimated_efficiency;
                    
                    task_single.task_echo.cleaned_path = task_single_item.task_echo.cleaned_path;
                    task_single.task_echo.coverage_rate = task_single_item.task_echo.coverage_rate;
                    task_single.task_echo.cleaned_area = task_single_item.task_echo.cleaned_area;
                    task_single.task_echo.real_average_speed = task_single_item.task_echo.real_average_speed;
                    task_single.task_echo.real_travel_dist = task_single_item.task_echo.real_travel_dist;
                }
            }    
        }
    }
    else{

    }
    UpdateTaskEcho(task_group.group_id, task_single);      //更新单个任务数据
    task_single.task_echo.end_time = __msg_convert.TimestampFormat(ros::Time::now().sec);
    cout << "[TaskSingleEndHandle | InsertTaskSingleRecord] --> task_id: " << task_single.task_base.task_id << endl;
    __task_db_ptr->InsertTaskSingleRecord(task_single, task_group.timertask_name, task_group.timertask_id);
}



// 任务组结束处理
void TaskManagerNode::TaskGroupEndHandle(TaskGroup task_group, bool need_record){
    LogPub("INFO", "组合任务结束，准备更新组合任务记录数据..., group_id: " + task_group.group_id
        + ", group_name: " + task_group.group_name
        + ", need_record: " + to_string(need_record)
        + ", timertask_name: " + task_group.timertask_name
        + ", timertask_id: " + task_group.timertask_id
    );
    // cout << "[TaskGroupEndHandle] need_record: " << need_record
    //     << ", timertask_name: " << task_group.timertask_name
    //     << ", timertask_id: " << task_group.timertask_id
    //     << endl;
    
    // 封装任务报告数据
    TaskRecord task_record;
    // task_record.status = TaskState::FINISHED;
    if(need_record){
        if(!task_group.task_list.empty()){
            if(IsChargingTask(task_group.task_list.front().task_base.task_type)
                || IsP2PTask(task_group.task_list.front().task_base.task_type)){
                task_record.error_code.error_msg = "[TaskGroupEndHandle] jump ChargingTask or P2PTask.";

            }
            else{
                vector<TaskRecord> task_records;
                TaskGroupRecord(task_group, false, task_records);
                cout << "[TaskGroupEndHandle] task_records size: " << task_records.size() << endl;
                if(task_records.empty()){
                    LogPub("ERROR", "[TaskGroupEndHandle]Can not find task record! group_id=" + task_group.group_id);
                    // task_record.error_code.error_msg = "Can not find task record.";
                    // task_record.status = TaskState::ERROR;
                }
                else{
                    task_record = task_records.front();
                    // cout << "[TaskGroupEndHandle] cleaned_area: " << to_string(task_record.cleaned_area)
                    //     << ", total_area: "  << to_string(task_record.total_area)
                    //     << ", coverage_rate: "  << to_string(task_record.coverage_rate)
                    //     << endl;
                    LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
                        + ", cleaned_area: " + to_string(task_record.cleaned_area)
                        + ", total_area: " + to_string(task_record.total_area)
                        + ", coverage_rate: " + to_string(task_record.coverage_rate)
                    );
                }
            }
        }
    }
    else{
        task_record.error_code.error_msg = "USER_CANCEL";
        task_record.status = TaskState::USER_CANCEL;
    }

    // 更新任务报告数据， 更新任务组状态
    // bool has_user_cancel = false;
    task_record.task_group = task_group;
    task_record.working_time = 0.0;         //修复时间叠加问题
    if(!task_group.task_list.empty()){
        task_record.start_time = task_group.task_list.front().task_echo.start_time;
        task_record.end_time = task_group.task_list.back().task_echo.end_time;
        task_record.error_code.error_msg = task_group.task_list.back().task_echo.error_code.error_msg;
        task_record.status = task_group.task_list.back().task_echo.task_status;
        for(auto& task_single : task_record.task_group.task_list){
            task_record.working_time += task_single.task_echo.working_time;
            // 避免存储信息过大，路径信息暂不存储。
            task_single.task_base.paths.clear();
            task_single.task_base.edge_paths.clear();
            task_single.task_echo.cleaned_path.poses.clear();
            // // 检索单个任务状态, 是否存在用户取消情况
            // if(task_single.task_echo.task_status == TaskState::USER_CANCEL){
            //     has_user_cancel = true;
            // }
        }        
    }
    // if(has_user_cancel){
    //     task_record.status = TaskState::USER_CANCEL;
    // }
    if(task_record.end_time.empty()){
        task_record.end_time = __msg_convert.TimestampFormat(ros::Time::now().sec);
    }
    task_record.uncleaned_area = task_record.total_area - task_record.cleaned_area;
    if(task_record.working_time > 0){
        // 按小时计算
        task_record.cleaning_efficiency = task_record.cleaned_area / (task_record.working_time / 3600);
    }
    else{
        task_record.start_time = task_record.end_time;
    }    
    // 图片字符串数据存储到数据库有问题，暂时不存储
    for(auto& task_picture : task_record.task_pictures){
        task_picture.png_str = "";
    }

    LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
        + ", start_time: " + task_record.start_time
        + ", end_time: " + task_record.end_time
        + ", cleaning_efficiency: " + to_string(task_record.cleaning_efficiency)
        + ", working_time: " + to_string(task_record.working_time)
        + ", task_pictures size: " + to_string(task_record.task_pictures.size())
    );

    // cout << "[TaskGroupEndHandle | InsertTaskRecord] --> group_name: " << task_record.task_group.group_name
    //     << ", group_id: " << task_record.task_group.group_id
    //     << ", timertask_name: " << task_record.task_group.timertask_name
    //     << ", timertask_id: " << task_record.task_group.timertask_id
    //     << ", start_time: " << task_record.start_time
    //     << ", end_time: " << task_record.end_time
    //     << ", cleaned_area: " << to_string(task_record.cleaned_area)
    //     << endl;    

    // 组合任务清洁报告处理

    // 覆盖率 >= 60: 
    //     状态 succes
    //     不存储原因
    // 覆盖率 < 60:
    //     状态 failed
    //     存储原因
    //         - 是强制结束，是则显示 "到达任务强制结束时间"
    //         - 是否用户取消，
    //             - 是 显示"任务被取消（手动取消）"
    //                 注：此原因优先级较高
    //             - 否，遍历普通任务的错误原因，存放集合中：
    //             - if 清洁时间>0 且清洁面积和覆盖率为0, 显示"清洁报告上传失败1"
    //             - else:
    //                    - 集合尺寸为空：
    //                        显示"清洁区域被占用"
    //                    - 集合尺寸为1个:
    //                        - 错误原因是否在已知列表中
    //                            - 存在：显示对应错误原因
    //                            - 不存在：显示"清洁报告上传失败2"
    //                    - 集合尺寸为多个：
    //                        - 以现场原因为准，显示"清洁区域被占用" 
                
    // 切换地图自动取消的任务，归属到手动取消

    bool is_cancel = false;
    bool is_assigned = false;
    set<long> error_code_list; 
    if(!task_group.task_list.empty()){
        for(auto& task_single : task_record.task_group.task_list){
            cout << "--> task_status: " << __msg_convert.TaskState2Str(task_single.task_echo.task_status) << endl;
            // 是否存在用户取消操作
            if(task_single.task_echo.task_status == TaskState::USER_CANCEL){
                is_cancel = true;
                task_record.status = TaskState::USER_CANCEL;    //更新为取消状态
            }
            else{
                if(task_single.task_echo.error_code.error_code > 0){
                    error_code_list.emplace(task_single.task_echo.error_code.error_code);
                }
            }
            // 此组合任务是否已指派下发
            if(task_single.task_base.is_assigned){
                is_assigned = true;
            }
        }
    }
    // 新增组合任务是否指派下发
    task_record.is_assigned = is_assigned;

    LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
        + ", is_cancel: " + to_string(is_cancel)
        + ", is_assigned: " + to_string(is_assigned)
        + ", coverage_rate: " + to_string(task_record.coverage_rate)
        + ", error_code_list size: " + to_string(error_code_list.size())
    );

    int language = ros::param::param<int>("/language", 0);
    if(task_record.coverage_rate * 100 >= __task_report_config.coverage_threshold){
        task_record.error_code.error_msg = "";
    }
    else{
        // 变更错误消息
        // 获取不同国家错误原因
        auto GetErrorMsg = [&](long error_code) -> string {
            string error_msg;
            if(__task_report_config.reasons_translation_list.count(error_code) > 0){
                if(language < __task_report_config.reasons_translation_list[error_code].size() && language >=0){
                    error_msg = __task_report_config.reasons_translation_list[error_code][language];
                }
            }
            return error_msg;
        };

        long error_code;
        // 是否用户取消
        if(is_cancel){
            error_code = __task_report_config.error_code_map.count("user_cancel") > 0 ? 
                __task_report_config.error_code_map["user_cancel"] : 0x050102;
        }
        else{
            if(task_record.working_time > 0 && task_record.coverage_rate < 0.01 && task_record.cleaned_area < 0.01){
                error_code = 0x050109;
            }
            else{
                if(error_code_list.size() == 1){
                    error_code = *(error_code_list.begin());
                    if(__task_report_config.reasons_translation_list.count(error_code) > 0){
                        ;
                    }
                    else{
                        error_code = 0x05010A;
                    }
                }
                // else if(error_code_list.empty()){
                //     if(task_record.working_time > 0 && task_record.coverage_rate < 0.01 && task_record.cleaned_area < 0.01){
                //         error_code = 0x05010A;
                //     }
                //     else{
                //         // 如果组合任务存在不同原因或者错误原因为空，以现场原因为准，统一报"清洁区域被占用"
                //         error_code = __task_report_config.error_code_map.count("area_occupied") > 0 ? 
                //             __task_report_config.error_code_map["area_occupied"] : 0x050102;                    
                //     }
                // }
                else{
                    // 如果组合任务存在不同原因或者错误原因为空，以现场原因为准，统一报"清洁区域被占用"
                    error_code = __task_report_config.error_code_map.count("area_occupied") > 0 ? 
                        __task_report_config.error_code_map["area_occupied"] : 0x050102;                    
                }
            }

        }
        task_record.error_code.error_code = error_code;
        task_record.error_code.error_msg = GetErrorMsg(error_code);

        LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
            + ", language: " + to_string(language)
            + ", error_code: " + to_string(error_code)
            + ", error_msg: " + task_record.error_code.error_msg
        );

        // cout << "[TaskGroupEndHandle] language: " << language 
        //     << ", is_assigned: " << is_assigned
        //     << ", is_cancel: " << is_cancel
        //     << ", error_code: " << task_record.error_code.error_code 
        //     << ", error_msg:" << task_record.error_code.error_msg.c_str() 
        //     << endl;
    }
    
    // 插入到数据库
    __task_db_ptr->InsertTaskRecord(task_record);
    LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
            + " insert task record to db, and ready to cloudsync......"
    );


    // 任务请求云同步
    auto TaskReportCloudSync = [&]() -> bool {
        v3_msgs::CloudSyncTaskReportManager client_data;
        client_data.request.task_record = __msg_convert.TaskRecord2V3Msgs(task_record);
        try{
            // 通知上传云平台
            if(!__cloudsync_record_client.call(client_data)){
                throw(string("/cloudsync_task_report_server call failed!"));
            } 
            // 解析返回数据
            if(!client_data.response.response.success){
                throw(client_data.response.response.message);
            } 
        }catch(string& error){
            return false;
        }  
        return true;
    };

    // 新增只有指派下发执行的任务报告才上传
    if(!task_group.task_list.empty()){
        if(IsChargingTask(task_group.task_list.front().task_base.task_type) ||
            IsP2PTask(task_group.task_list.front().task_base.task_type)){
            ;   //充电任务不上传
        }
        else{
            if(is_assigned) 
                TaskReportCloudSync();
        }
    }
    else{
        if(is_assigned)
            TaskReportCloudSync();       
    }

    LogPub("INFO", "[TaskGroupEndHandle] group_id: " + task_group.group_id
            + ", cloudsync finished. is_assigned: "  + to_string(is_assigned));
}
